#pragma once

#ifndef _MATH_H_
#define _MATH_H_

#include "Platform.h"
#include <math.h>
#include "VI_ValidNumber.h"

///////////////////////////////////////////////////////////////////////////////
// Forward declarations                                                      //
///////////////////////////////////////////////////////////////////////////////
template <typename F> struct Vector2_T;
template <typename F> struct Vector3_T;
template <typename F> struct Vector4_T;

///////////////////////////////////////////////////////////////////////////////
// Definitions                                                               //
///////////////////////////////////////////////////////////////////////////////
#define F_PI			float32(3.14159265358979323846264338327950288419716939937510)
#define F_TWO_PI		float32(3.14159265358979323846264338327950288419716939937510 * 2.0)
#define F_HALF_PI		float32(3.14159265358979323846264338327950288419716939937510 * 0.5)
#define R_PI			float64(3.14159265358979323846264338327950288419716939937510)
#define R_TWO_PI		float64(3.14159265358979323846264338327950288419716939937510 * 2.0)
#define R_HALF_PI		float64(3.14159265358979323846264338327950288419716939937510 * 0.5)

#define MAX(a,b)    (((a) > (b)) ? (a) : (b))
#define MIN(a,b)    (((a) < (b)) ? (a) : (b))

#define DEG2RAD( a ) ( (a) * (F_PI/180.0f) )
#define RAD2DEG( a ) ( (a) * (180.0f/F_PI) )

#define TANGENT30		0.57735026918962576450914878050196f // tan(30)
#define TWO_TANGENT30	0.57735026918962576450914878050196f*2 // 2*tan(30)

#define LN2 0.69314718055994530941723212145818f // ln(2)

template<class T> inline void limit_T(T& val, const T& min, const T& max)
{
	if (val < min) val = min;
	else if (val > max)	val = max;
}

template<class T> ILINE T clamp_T( T X, T Min, T Max ) 
{	
	return X<Min ? Min : X<Max ? X : Max; 
}

ILINE float32 vi_clamp( float32 X, float32 Min, float32 Max ) 
{	
	X = (X+Max-fabsf(X-Max))*0.5f;  //return the min
	X = (X+Min+fabsf(X-Min))*0.5f;  //return the max 
	return X;
}
ILINE float64 vi_clamp( float64 X, float64 Min, float64 Max ) 
{	
	X = (X+Max-fabs(X-Max))*0.5;  //return the min
	X = (X+Min+fabs(X-Min))*0.5;  //return the max 
	return X;
}

#if defined(_CPU_X86) && defined(_MSC_VER) && !defined(_DEBUG) && !defined(LINUX)

// calculates the cosine and sine of the given angle in radians 
ILINE void vi_sincosf (float32 angle, float32* pSin, float32* pCos) {
	__asm {
			FLD         DWORD PTR       angle
			FSINCOS
			MOV         eAX,pCos
			MOV         eDX,pSin
			FSTP        DWORD PTR [eAX]	//store cosine
			FSTP        DWORD PTR [eDX]	//store sine
	}
}

// calculates the cosine and sine of the given angle in radians 
ILINE void vi_sincos (float64 angle, float64* pSin, float64* pCos) {
	__asm {
			FLD         QWORD PTR       angle
			FSINCOS
			MOV         eAX,pCos
			MOV         eDX,pSin
			FSTP        QWORD PTR [eAX]	//store cosine
			FSTP        QWORD PTR [eDX]	//store sine
	}
}

ILINE float32 CorrectInvSqrt(float32 fNum, float32 fInvSqrtEst)
{
	// Newton-Rhapson method for improving estimated inv sqrt.
	/*
		f(x) = x^(-1/2)
		f(n) = f(a) + (n-a)f'(a)
				 = a^(-1/2) + (n-a)(-1/2)a^(-3/2)
				 = a^(-1/2)*3/2 - na^(-3/2)/2
				 = e*3/2 - ne^3/2
	*/
	return fInvSqrtEst * (1.5f - fNum * fInvSqrtEst * fInvSqrtEst * 0.5f);
}

#include <xmmintrin.h>

ILINE float32 vi_sqrtf(float32 x) 
{
	__m128 s = _mm_sqrt_ss(_mm_set_ss(x));
	float r; _mm_store_ss(&r, s); 
	return r;
}
ILINE float32 vi_sqrtf_fast(float32 x)
{
	__m128 s = _mm_rcp_ss(_mm_rsqrt_ss(_mm_set_ss(x)));
	float r; _mm_store_ss(&r, s); 
	return r;
}
ILINE float32 vi_isqrtf(float32 x) 
{
	__m128 s = _mm_rsqrt_ss(_mm_set_ss(x));
	float r; _mm_store_ss(&r, s);
	return CorrectInvSqrt(x, r);
}
ILINE float32 vi_isqrtf_fast(float32 x) 
{
	__m128 s = _mm_rsqrt_ss(_mm_set_ss(x));
	float r; _mm_store_ss(&r, s);
	return r;
}

#else

ILINE void vi_sincosf (float32 angle, float32* pSin, float32* pCos) {	*pSin = float32(sin(angle));	*pCos = float32(cos(angle));	}
ILINE void vi_sincos  (float64 angle, float64* pSin, float64* pCos) {	*pSin = float64(sin(angle));  *pCos = float64(cos(angle));	}
ILINE float32 vi_sqrtf(float32 x) {return sqrtf(x);}
ILINE float32 vi_sqrtf_fast(float32 x) {return sqrtf(x);}
ILINE float32 vi_isqrtf(float32 x) {return 1.0f/sqrtf(x);}
ILINE float32 vi_isqrtf_fast(float32 x) {return 1.f/sqrtf(x);}

#endif

ILINE float32 vi_sinf(float32 x) {return sinf(x);}
ILINE float32 vi_cosf(float32 x) {return cosf(x);}
ILINE float32 vi_asinf(float32 x) {return asinf( vi_clamp(x,-1.0f,+1.0f) );}
ILINE float32 vi_acosf(float32 x) {return acosf( vi_clamp(x,-1.0f,+1.0f) );}
ILINE float32 vi_atanf(float32 x) {return atanf(x);}
ILINE float32 vi_atan2f(float32 y, float32 x) {return atan2f(y,x);}
ILINE float32 vi_tanf(float32 x) {return tanf(x);}

ILINE float32 vi_fmod(float32 x, float32 y) {return (float32)fmodf(x,y);}
ILINE float32 vi_ceilf(float32 x) {return ceilf(x);}
ILINE float32 vi_tanhf(float32 x) {return tanhf(x);}
ILINE float32 vi_fabsf(float32 x) {return fabsf(x);}
ILINE float32 vi_expf(float32 x) {return expf(x);}
ILINE float32 vi_logf(float32 x) {return logf(x);}
ILINE float32 vi_floorf(float32 x) {return floorf(x);}
ILINE float32 vi_powf(float32 x, float32 y) {return powf(x,y);}

ILINE void sincos_T(float64 angle, float64* pSin, float64* pCos) { vi_sincos(angle, pSin, pCos); }
ILINE void sincos_T(float32 angle, float32* pSin, float32* pCos) { vi_sincosf(angle, pSin, pCos); }

ILINE float64 cos_T(float64 op) { return cos(op); }
ILINE float64 sin_T(float64 op) { return sin(op); }
ILINE float64 acos_T(float64 op) { return acos( vi_clamp(op,-1.0,+1.0) ); }
ILINE float64 asin_T(float64 op) { return asin( vi_clamp(op,-1.0,+1.0) ); }
ILINE float64 atan_T(float64 op) { return atan(op); }
ILINE float64 atan2_T(float64 op1, float64 op2) { return atan2(op1,op2); }
ILINE float64 tan_T(float64 op) {return tan(op);}

ILINE float32 cos_T(float32 op) { return vi_cosf(op); }
ILINE float32 sin_T(float32 op) { return vi_sinf(op); }
ILINE float32 acos_T(float32 op) { return vi_acosf(op); }
ILINE float32 asin_T(float32 op) { return vi_asinf(op); }
ILINE float32 atan_T(float32 op) { return vi_atanf(op); }
ILINE float32 atan2_T(float32 op1, float32 op2) { return vi_atan2f(op1,op2); }
ILINE float32 tan_T(float32 op) {return vi_tanf(op);}

ILINE float64 exp_T(float64 op) { return exp(op); }
ILINE float32 exp_T(float32 op) { return vi_expf(op); }

ILINE float64 log_T(float64 op) { return log(op); }
ILINE float32 log_T(float32 op) { return vi_logf(op); }

ILINE float64 sqrt_T(float64 op) { return sqrt(op); }
ILINE float32 sqrt_T(float32 op) { return vi_sqrtf(op); }

// Only f32 version implemented, as it's approximate.

ILINE float32 sqrt_fast_T(float32 op) { return vi_sqrtf_fast(op); }

ILINE float64 isqrt_T(float64 op) { return 1.0/sqrt(op); }
ILINE float32 isqrt_T(float32 op) { return vi_isqrtf(op); }

ILINE float32 isqrt_fast_T(float32 op) { return vi_isqrtf_fast(op); }

ILINE float64 isqrt_safe_T(float64 op) { return 1.0/sqrt(op + DBL_MIN); }
ILINE float32 isqrt_safe_T(float32 op) { return vi_isqrtf(op + FLT_MIN); }

ILINE float64 fabs_T(float64 op) { return fabs(op); }
ILINE float32 fabs_T(float32 op) { return vi_fabsf(op); }
ILINE int32 fabs_T(int32 op) { int32 mask=op>>31; return op+mask^mask; }

ILINE int32 floor_T(int32 op) {return op;}
ILINE float32 floor_T(float32 op) {return vi_floorf(op);}
ILINE float64 floor_T(float64 op) {return floor(op);}

ILINE int32 ceil_T(int32 op) {return op;}
ILINE float32 ceil_T(float32 op) {return vi_ceilf(op);}
ILINE float64 ceil_T(float64 op) {return ceil(op);}

// Fast float-int rounding functions.
#if defined(WIN32) && defined(_CPU_X86)
ILINE int32 int_round(float32 f)
{
	int32 i;
	__asm fld [f]
	__asm fistp [i]
	return i;
}
ILINE int32 pos_round(float32 f)  { return int_round(f); }
#else
ILINE int32 int_round(float32 f)  { return f < 0.f ? int32(f-0.5f) : int32(f+0.5f); }
ILINE int32 pos_round(float32 f)  { return int32(f+0.5f); }
#endif

ILINE int64 int_round(float64 f)  { return f < 0.0 ? int64(f-0.5) : int64(f+0.5); }
ILINE int64 pos_round(float64 f)  { return int64(f+0.5); }

ILINE int32 int_ceil(float32 f)
{
	int32 i = int32(f);
	return (f > float32(i)) ? i+1 : i;
}

ILINE int64 int_ceil(float64 f)
{
	int64 i = int64(f);
	return (f > float64(i)) ? i+1 : i;
}

// Fixed-int to float conversion.
ILINE float ufrac8_to_float( float u )
{
	return u * (1.f/255.f);
}
ILINE float ifrac8_to_float( float i )
{
	return i * (1.f/127.f);
}
ILINE uint8 float_to_ufrac8( float f )
{
	int i = pos_round(f * 255.f);
	//assert(i >= 0 && i < 256);
	return uint8(i);
}
ILINE int8 float_to_ifrac8( float f )
{
	int i = int_round(f * 127.f);
	//assert(abs(i) <= 127);
	return int8(i);
}

//inline int32 incm3(int32 i) { return (i+1 & (i-2))>>31; }
//inline int32 decm3(int32 i) { return i-1 + ((i-1)>>31&3); }

template<class F> inline F square(F fOp) { return(fOp*fOp); }
template<class F> inline F sqr(const F &op) { return op*op; }
template<class F> inline F cube(const F &op) { return op*op*op; }
template<class F> inline F sqr_signed(const F &op) { return op * fabs_T(op); }

//if x is negative return -1, else return 1.
ILINE int32 sgnnz(float64 x)
{
	union
	{
		float32 f;
		int32 i;
	} u;
	u.f=(float32)x;
	return ((u.i>>31)<<1)+1;
}

ILINE int32 sgnnz(float32 x)
{
	union
	{
		float32 f;
		int32 i;
	} u;
	u.f=x;
	return ((u.i>>31)<<1)+1;
}	

ILINE int32 sgnnz(int32 x)
{
	return ((x>>31)<<1)+1;
}

ILINE float fsgnnz(float32 x)
{
	union { float32 f; int32 i; } u;
	u.f=x;
	u.i=u.i & 0x80000000 | 0x3f800000;
	return u.f;
}

//if x is negative return 1, else return 0.
ILINE int32 isneg(float64 x)
{
	union
	{
		float32 f;
		uint32 i;
	} u;
	u.f=(float32)x;
	return (int32)(u.i>>31);
}

ILINE int32 isneg(float32 x)
{
	union
	{
		float32 f;
		uint32 i;
	} u;
	u.f=x;
	return (int32)(u.i>>31);
}

ILINE int32 isneg(int32 x)
{
	return (int32)((uint32)x>>31);
}

//if x is negative return -1, else if is zero return 0, else return 1.
ILINE int32 sgn(float64 x)
{
	union
	{
		float32 f;
		int32 i;
	} u;
	u.f=(float32)x;
	return (u.i>>31)+((u.i-1)>>31)+1;
}

ILINE int32 sgn(float32 x)
{
	union
	{
		float32 f;
		int32 i;
	} u;
	u.f=x;
	return (u.i>>31)+((u.i-1)>>31)+1;
}

ILINE int32 sgn(int32 x)
{
	return (x>>31)+((x-1)>>31)+1;
}

//if x is negative return 0, else return 1.
ILINE int32 isnonneg(float64 x)
{
	union
	{
		float32 f;
		uint32 i;
	} u;
	u.f=(float32)x;
	return (int32)(u.i>>31^1);
}

ILINE int32 isnonneg(float32 x)
{
	union
	{
		float32 f;
		uint32 i;
	} u;
	u.f=x;
	return (int32)(u.i>>31^1);
}

ILINE int32 isnonneg(int32 x)
{
	return (int32)((uint32)x>>31^1);
}

//if x is zero return 1, else return 0.
ILINE int32 iszero(float64 x)
{
	union
	{
		float32 f;
		int32 i;
	} u;
	u.f=(float32)x;
	u.i&=0x7FFFFFFF;
	return -((u.i>>31)^(u.i-1)>>31);
}

ILINE int32 iszero(float32 x)
{
	union
	{
		float32 f;
		int32 i;
	} u;
	u.f=x;
	u.i&=0x7FFFFFFF;
	return -(u.i>>31^(u.i-1)>>31);
}

ILINE int32 iszero(int32 x)
{
	return -(x>>31^(x-1)>>31);
}

//////////////////////////////////////////////////////////////////////////
// Define min/max
//////////////////////////////////////////////////////////////////////////
#ifdef min
#undef min
#endif //min

#ifdef max
#undef max
#endif //max

// Bring min and max from std namespace to global scope.
template <class T, class U>
inline T min( const T& a, const U& b )
{
	return b < a ? (T)b : a;
}

template <class T, class U>
inline T max( const T& a, const U& b )
{
	return a < b ? (T)b : a;
}

template <class T, class _Compare>
inline const T& min( const T& a, const T& b, _Compare comp)
{
	return comp(b,a) ? b : a;
}

template <class T, class _Compare>
inline const T& max( const T& a, const T& b, _Compare comp)
{
	return comp(a,b) ? b : a;
}

enum type_zero { ZERO };
enum type_min { VMIN };
enum type_max { VMAX };
enum type_identity { IDENTITY };

#include "VI_Vector2.h"
#include "VI_Vector3.h"
#include "VI_Quaternion.h"
#include "VI_Matrix.h"

#endif//_MATH_H_
